#include <vector>
#include <stack>
#include <algorithm>
#include <stdlib.h>

#include <osg/Vec3d>
#include "OsgUtil.h"

std::vector<osg::Vec3d> convexHull(const std::vector<osg::Vec3d>& points);
// points必须在同一平面,返回points的凸包，点的顺序不变
bool getConvexHull(const std::vector<osg::Vec3d> &points, std::vector<osg::Vec3d>& hullPoints)
{
	if (points.size() < 3)
		return false;
	osg::Matrixd matrix;
	bool success = getMatrix(points[0], points[1], points[2], matrix);
	if (!success)
		return false;
	auto matrix_1 = osg::Matrixd::inverse(matrix);
	// 得到局部平面坐标系上坐标
	std::vector<osg::Vec3d> points2d;
	for (const auto& pt : points)
	{
		auto pt2d = pt * matrix_1;
		points2d.push_back(pt2d);
	}
	// 得到闭包
	auto hull = convexHull(points2d);
	// 对闭包点排序
	std::vector<int> pointIndice;
	std::vector<osg::Vec3d> temp;
	for (const auto& pt : hull)
	{
		auto pt3d = pt * matrix;
		for (int i = 0; i < points.size(); ++i)
		{
			if (pointApprox(pt3d, points[i]))
			{
				pointIndice.push_back(i);
				break;
			}
		}
	}

	if (pointIndice.size()<3)
	{
		int i = 0;
	}
	std::sort(pointIndice.begin(), pointIndice.end());
	for (auto index : pointIndice)
	{
		hullPoints.push_back(points[index]);
	}
	return true;
}


double isLeft(const osg::Vec3d& P0, const osg::Vec3d& P1, const osg::Vec3d& P2)
{
	return (P1.x() - P0.x())*(P2.y() - P0.y()) - (P2.x() - P0.x())*(P1.y() - P0.y());
}

// simpleHull_2D(): Melkman's 2D simple polyline O(n) convex hull algorithm
 // z() is not used.
 std::vector<osg::Vec3d> convexHull(const std::vector<osg::Vec3d>& points)//Point* P, int n, Point* H
{
	// initialize a deque D[] from bottom to top so that the
	// 1st three vertices of P[] are a ccw triangle
	 std::vector<osg::Vec3d> deque;
	 std::vector<osg::Vec3d> result;
	int pointsSize = points.size();
	deque.resize(2 * pointsSize + 1);
	int bot = pointsSize - 2, top = bot + 3;    // initial bottom and top deque indices
	deque[bot] = deque[top] = points[2];        // 3rd vertex is at both bot and top
	if (isLeft(points[0], points[1], points[2]) > 0)
	{
		deque[bot + 1] = points[0];
		deque[bot + 2] = points[1];           // ccw vertices are: 2,0,1,2
	}
	else
	{
		deque[bot + 1] = points[1];
		deque[bot + 2] = points[0];           // ccw vertices are: 2,1,0,2
	}

	// compute the hull on the deque D[]
	for (int i = 3; i < pointsSize; i++)
	{   // process the rest of vertices
		// test if next vertex is inside the deque hull
		if ((isLeft(deque[bot], deque[bot + 1], points[i]) > 0) &&
			(isLeft(deque[top - 1], deque[top], points[i]) > 0))
			continue;         // skip an interior vertex

   // incrementally add an exterior vertex to the deque hull
   // get the rightmost tangent at the deque bot
		while (isLeft(deque[bot], deque[bot + 1], points[i]) <= 0)
			++bot;                 // remove bot of deque
		deque[--bot] = points[i];           // insert P[i] at bot of deque

		// get the leftmost tangent at the deque top
		while (isLeft(deque[top - 1], deque[top], points[i]) <= 0)
			--top;                 // pop top of deque
		deque[++top] = points[i];           // push P[i] onto top of deque
	}

	// transcribe deque D[] to the output hull array H[]
	int h;        // hull vertex counter
	for (h = 0; h < (top - bot); h++)
		result.push_back(deque[bot + h]) ;
	return result;
}
